#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray
import argparse
import time

class MoveJointCommander(Node):
    def __init__(self, joint_positions):
        super().__init__('move_joint_commander')
        self.publisher_ = self.create_publisher(Float64MultiArray, '/command_move_joint', 10)
        self.joint_positions = joint_positions

    def wait_for_subscribers(self, timeout=3.0):
        self.get_logger().info("Waiting for subscribers to connect...")
        start_time = time.time()
        while self.publisher_.get_subscription_count() == 0:
            if time.time() - start_time > timeout:
                self.get_logger().warn("Timeout waiting for subscribers!")
                return False
            rclpy.spin_once(self, timeout_sec=0.1)
            time.sleep(0.1)
        self.get_logger().info("Subscribers connected.")
        return True

    def publish_joint_command(self):
        msg = Float64MultiArray()
        if len(self.joint_positions) == 7:
            msg.data = self.joint_positions
            self.get_logger().info(f'Moving to joint positions: {msg.data}')
        else:
            msg.data = [0.0] * 7
            self.get_logger().info('No arguments provided. Moving to home position [0, 0, 0, 0, 0, 0, 0]')

        self.publisher_.publish(msg)
        self.get_logger().info("Published joint command.")

def main():
    parser = argparse.ArgumentParser(description='Set joint positions.')
    parser.add_argument('joint_positions', type=float, nargs='*',
                        help='7 joint positions to command (if none provided, moves to home position)')
    args = parser.parse_args()

    rclpy.init()
    node = MoveJointCommander(args.joint_positions)

    try:
        if node.wait_for_subscribers():
            node.publish_joint_command()
            # 给ROS一些时间完成消息发送
            for _ in range(10):
                rclpy.spin_once(node, timeout_sec=0.1)
                time.sleep(0.1)
        else:
            node.get_logger().warn("No subscribers connected, aborting publish.")
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()
